/**
 * @author Jerome Monceaux
 * @version 1.0
 * 
 * GenericVirtualMotor : This class allow differential drive motor simulation.
 **/
package org.hh.locodo;

import org.hh.locodo.MotorInterface;

import ioio.lib.api.DigitalOutput;
import ioio.lib.api.IOIO;
import ioio.lib.api.PwmOutput;
import ioio.lib.api.DigitalOutput.Spec.Mode;
import ioio.lib.api.exception.ConnectionLostException;
import ioio.lib.api.TwiMaster;


/**
 * 
 * @author bilock
 * Communication with encoder and motor using a ioio board.
 * Here we used 2 io pins from the ioio board to control the motor :
 * - 1 to deliver pwm signal
 * - 1 to control the rotation direction of the wheel.
 * 
 * The encoder is a ME210 : Mangetic Rotary encoder sells by acroname.
 * It is drive using the i2c interface.
 * 
 * We are using the 5V open drain mode with a pull up resistor for the output pwm.
 * But unfortunately, it give 5V by default at startup so the motor goes full speed.
 * To avoid this effect a 5V NOT gate is suppose to invert the pwm. The pwm duty cycle 
 * and the sign of the directional output suppose the presence of this gate.
 * 
 */
public class IoioMotorControler implements MotorInterface {

  private double fDesiredSpeed;	// new speed value 
  private double fSpeed;				// current speed value
  private int fEncVal;					// encoder value 
  private int fPwmPinNumber;		// pwm pin id
  private int fSignPinNumber;		// directional pin id
  private int fI2CDeviceAdress;	// i2c adress of the encoder.
	
  final static byte[] REG_RESET = new byte[] {0x52};			// Reset register of the ME210
  final static byte[] REG_GETDISTANCE = new byte[] {0x44};// GetDistance Command Register
  
	// Encoder value response storage
	private byte[] fEncResponse = new byte[4+1];
	
	private IOIO fIoio;									// handle to ioio board interface
	private TwiMaster fTwi;							// handle to i2c interface
	private PwmOutput fPwmOutput;				// handle to pwm output
	private DigitalOutput fSignOutput;	// handle to direction booleab pin

	
  public IoioMotorControler(int pwmPin, int signPin, int i2CDeviceAdress, IOIO ioio, TwiMaster twi) throws ConnectionLostException {
		fPwmPinNumber = pwmPin;
		fSignPinNumber = signPin;
	  fDesiredSpeed = 0.f;
	  fSpeed = 0.f;
	  fEncVal = 0;
	  fIoio = ioio;
	  fI2CDeviceAdress = i2CDeviceAdress;
	  
		fPwmOutput = fIoio.openPwmOutput(new DigitalOutput.Spec(fPwmPinNumber, Mode.OPEN_DRAIN), 30000);
		fSignOutput = fIoio.openDigitalOutput(new DigitalOutput.Spec(fSignPinNumber, Mode.OPEN_DRAIN), false);
		
		fTwi = twi;
		
		//set motor speed to 0
		fPwmOutput.setDutyCycle(1.f); // suppose the NOT gate
		
		//Reset Encoder
		try {
			byte[] emptyResp = new byte[] {0}; // workaround
			fTwi.writeRead(fI2CDeviceAdress, false, REG_RESET, REG_RESET.length, emptyResp, 0);
		}catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
	}
	
	public void close()
	{
		if( fPwmOutput != null )
			fPwmOutput.close();
	} 
	
	@Override
	synchronized public int getEncoderValue() {
		return fEncVal;
	}

	@Override
	public void setSpeed(double speed) {
		fDesiredSpeed = speed;
	}

	@Override
	public void update() throws ConnectionLostException{
	  float s = (float) fSpeed;
		fTwi.writeReadAsync(fI2CDeviceAdress, false, REG_GETDISTANCE, REG_GETDISTANCE.length, fEncResponse, fEncResponse.length);

		if( fSpeed > 0) // uses inversion bit to configure the H bridge 
			fSignOutput.write(false);
		else{
			fSignOutput.write(true);
			s = (float) -fSpeed;
		}

		if( fDesiredSpeed != fSpeed ){
	  	fPwmOutput.setDutyCycle(1.f - s); // suppose 5V NOT gate
	  	fSpeed = fDesiredSpeed;
	  }
	  
	  //res.waitReady(); // do not wait for acknowledge, otherwise cycle time will explode
	  synchronized(this){
			fEncVal = byteArrayToInt(fEncResponse);
	  }
	}
	
  private static int byteArrayToInt(byte[] b){ // byte rearrangement
  	int value = ((b[4] & 0xff) << 24) | ((b[3] & 0xff) << 16) | ((b[2] & 0xff) << 8) | (b[1] & 0xff);
    return value;
  }
}
